/*  
 *  Copyright RobOtter (2008) 
 * 
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

/** \file hrobot_system.h
  *
  * Abstraction layer between hardware and control systems
  */

#ifndef HROBOT_SYSTEM_H
#define HROBOT_SYSTEM_H

#include <aversive.h>
#include <vectors.h>
#include "hrobot_config.h"

typedef struct hrobot_system
{

  //________________________________

  /* Encoders */
  void (*encoder_CO1)(void*, vxys_t*);
  void* CO1_params;

  void (*encoder_CO2)(void*, vxys_t*);
  void* CO2_params;

  void (*encoder_CO3)(void*, vxys_t*);
  void* CO3_params;

  /* Motors */
  void (*motor_M1)(void*, int32_t);
  void* M1_params;
  int32_t M1_sign;

  void (*motor_M2)(void*, int32_t);
  void* M2_params;
  int32_t M2_sign;

  void (*motor_M3)(void*, int32_t);
  void* M3_params;
  int32_t M3_sign;

  //________________________________

  vxys_t CO1_value;
  vxys_t CO2_value;
  vxys_t CO3_value;

}hrobot_system;

// ______________
//   SETUP

/**\brief Fill HRS structure with the function which access encoder values */
void hrs_set_CO1_encoder(hrobot_system* hrs,
                          void (*f)(void*,vxys_t*),void* p);

/**\brief Fill HRS structure with the function which access encoder values */
void hrs_set_CO2_encoder(hrobot_system* hrs,
                          void (*f)(void*,vxys_t*),void* p);

/**\brief Fill HRS structure with the function which access encoder values */
void hrs_set_CO3_encoder(hrobot_system* hrs,
                          void (*f)(void*,vxys_t*), void* p);


/**\brief Fill HRS structure with the function which control motor */
void hrs_set_M1_motor(hrobot_system* hrs,
                        void (*f)(void*,int32_t), void* p);

/**\brief Fill HRS structure with the function which control motor */
void hrs_set_M2_motor(hrobot_system* hrs, 
                        void (*f)(void*,int32_t), void* p);

/**\brief Fill HRS structure with the function which control motor */
void hrs_set_M3_motor(hrobot_system* hrs,
                        void (*f)(void*,int32_t), void* p);

// _______________
//    COMPUTE

/**\brief Zero structure */
void hrs_init(hrobot_system* hrs);

/**
* \brief Read the encoders and update values.
* \note hrs_update take void* to be used without warnings
*  in scheduler.
* Internal values are computed depending on encoders
* quality estimation and other parameters.
* 
*/
void hrs_update(void*);

// ______________
//   ACCESSORS

/**\brief Return CO1 encoder value */
vxys_t hrs_get_CO1(hrobot_system* hrs);

/**\brief Return CO2 encoder value */
vxys_t hrs_get_CO2(hrobot_system* hrs);

/**\brief Return CO3 encoder value */
vxys_t hrs_get_CO3(hrobot_system* hrs);


/**\brief Set M1 motor consign */
void hrs_set_M1(hrobot_system* hrs,int32_t v);

/**\brief Set M2 motor consign */
void hrs_set_M2(hrobot_system* hrs,int32_t v);

/**\brief Set M3 motor consign */
void hrs_set_M3(hrobot_system* hrs,int32_t v);

/**\brief Set motors consign sign */
void hrs_set_motors_signs(hrobot_system* hrs,
                            int32_t s1, int32_t s2, int32_t s3);

// _______________
//    ORDERS




#endif/*HROBOT_SYSTEM_H*/
